#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include "itpack2d.h"

float grad(float *data, int columns, int rows, int k,int l)
{
  // A = (sqrt(2)-1)/(2-sqrt(2))
  // B = 1/(2+4*A)
  // C = A*B
  float C = 0.14645, B = 0.20711;
  float x,y, r;
  x = ( -C*data[k*columns+l - columns -1] 
	-B*data[k*columns+l - columns]
	-C*data[k*columns+l - columns +1]
	+C*data[k*columns+l + columns -1] 
	+B*data[k*columns+l + columns]
	+C*data[k*columns+l + columns +1]);
  y = ( -C*data[k*columns+l -1 - columns] 
	-B*data[k*columns+l -1]
	-C*data[k*columns+l -1 + columns]
	+C*data[k*columns+l +1 - columns] 
	+B*data[k*columns+l +1]
	+C*data[k*columns+l +1 + columns]);
  r = sqrt(x*x+y*y);
  return r;
}


float lambda1 = 1.0;
float lambda2 = 2.0;
float tau = 0.5;
//float delta = 1.0;
float dt = 0.1;
float epsilon = 0.2;
float nu = 0.0;
float rmu = 0.02;
float mu;

float ZEE = 0.000001;

#define H(z)          (0.5+ 1.0/M_PI*atanf((z)/epsilon))
#define DELTA(z)      epsilon/M_PI/(epsilon*epsilon + (z)*(z))

#define JCOEF(i,j) (jcoef[(j)*(N)+(i)])
#define COEF(i,j) (coef[(j)*(N)+(i)])
#define NOFFSET(k,l,x,y) (((k)+(x))*(columns)+(l)+(y))
#define JOFFSET(k,l,j) (((k)-1+(j)/3)*(columns)+(l)-1+(j)%3)
#define OFFSET(k,l) ((k)*columns+(l))

int diffuse(float *u, float *phi, int columns, int rows)
{
  int t,MAXT = 20;
  int N = columns*rows, NDIM=N, MAXNZ=9;
  int ITMAX = 200, NW=6*N+4*ITMAX, IER;
  float *coef, *gcoef, *WKSP, RPARM[12], *RHS;
  int *jcoef, *IWKSP, IPARM[12];
  int i,j,k,l;

  float c1,c2, omega1, omega2,delta;
  float *px, *py,temp,h;
  
      fprintf(stderr,"Here\n");

 
  jcoef = malloc(sizeof(int) * N * MAXNZ);
  coef = malloc(sizeof(float) * N * MAXNZ);
  gcoef = malloc(sizeof(float) * N * MAXNZ);
  RHS = malloc(sizeof(float) * N);
  px = malloc(sizeof(float) * N); //phi x
  py = malloc(sizeof(float) * N); //phi y

  WKSP = malloc(sizeof(float) * NW);
  IWKSP = malloc(sizeof(int) * N * 3);
  
      fprintf(stderr,"Here\n");

  dfault_(IPARM, RPARM);
  IPARM[0] = ITMAX;
  IPARM[1] = 1; //output level
  IPARM[4] = 2; // nonsymmetic
  IPARM[11] = 1; //print DIGIT1 and DIGIT2
  
  mu = rmu * columns * rows;

  for(l=0;l<columns;l++)
    {
      JCOEF(l,0) = l+1;
      JCOEF(N-l-1,0) = N-l;
      COEF(l, 0) = COEF(N-l-1,0) =  1.;
      for(j=1;j<MAXNZ;j++)
	JCOEF(l,j) = JCOEF(N-l-1,j) = 0;
    }
  
  for(k=1;k<rows-1;k++)
    {
      JCOEF(k*columns,0) = k*columns+1;
      JCOEF(k*columns+columns-1,0) = k*columns+columns;
      COEF(k*columns,0) = COEF(k*columns+columns-1,0) = 1.;
      for(j=1;j<MAXNZ;j++)
	JCOEF(k*columns,j) = JCOEF(k*columns+columns-1,j) = 0;
    }
  
  // Note: SSORCG will sort the JCOEF and COEF in its own order.
  // So the calling order must desinged well
  // First JCOEF and GCOEF is in my own order, 
  // then run ssorcg, GCOEF's order changes.
  // Reset JCOEF and COEF to my own order, then run ssorcg.
  // Now JCOEF's order is SSORCG's, in the next iteration, 
  // GCOEF will not change, and JCOEF and GCOEF are both SSORCG's order
  // So the follow program can work.
  
  for(t = 0; t<MAXT; t++)
    {
      c1 = c2 = omega1 = omega2=0;
      for(k=0;k<rows;k++)
	for(l=0;l<columns;l++)
	  {
	    h = H(phi[OFFSET(k,l)]);
	    c1 += h*u[OFFSET(k,l)];
	    c2 += (1.0-h) * u[OFFSET(k,l)];
	    omega1 += h;
	    omega2 += (1.0-h);
	  }
      c1 /= omega1;
      c2 /= omega2;
            
      memcpy(RHS, phi, sizeof(float)*N);
  
      for(k=1;k<rows-1;k++)
	for(l=1;l<columns-1;l++)
	  {
	    for(j=0;j<MAXNZ;j++)
	      JCOEF(OFFSET(k,l),j) = JOFFSET(k,l,j) + 1;
	  }

      // See Active Contours Without Edges, Tony F.Chan
      for(k=1;k<rows-1;k++)
	for(l=1;l<columns-1;l++)
	  {
	    temp = 
	      sqrt((phi[OFFSET(k,l+1)]-phi[OFFSET(k,l)])
		   *(phi[OFFSET(k,l+1)]-phi[OFFSET(k,l)])
		   +(phi[OFFSET(k,l+1)]-phi[OFFSET(k,l-1)])
		   *(phi[OFFSET(k,l+1)]-phi[OFFSET(k,l-1)])/2);
	    if(isnanf(temp)) temp=0.0;
	    px[OFFSET(k,l)] = 1/(temp+ZEE);
	    temp = 
	      sqrt((phi[OFFSET(k+1,l)]-phi[OFFSET(k-1,l)])
		   *(phi[OFFSET(k+1,l+1)]-phi[OFFSET(k-1,l)])/2
		   +(phi[OFFSET(k+1,l)]-phi[OFFSET(k,l)])
		   *(phi[OFFSET(k+1,l)]-phi[OFFSET(k,l)]));
	    //fprintf(stderr,"%5.4f ",temp);
	    if(isnanf(temp)) temp=0.0;
	    py[OFFSET(k,l)] = 1/(temp+ZEE);

	  }
      for(k=1;k<rows-1;k++)
	{
	  temp = 
	    sqrt((phi[OFFSET(k,1)]-phi[OFFSET(k,0)])
		 *(phi[OFFSET(k,1)]-phi[OFFSET(k,0)])*2);
	  if(isnanf(temp)) temp=0.0;


	  px[OFFSET(k,0)] = 1/(temp+ZEE);
	  temp = 
	    sqrt((phi[OFFSET(k,columns-1)]-phi[OFFSET(k,columns-2)])
		 *(phi[OFFSET(k,columns-1)]-phi[OFFSET(k,columns-2)])*2);
	  if(isnanf(temp)) temp=0.0;

	  px[OFFSET(k,columns-1)] = 1/(temp+ZEE);
	}

      for(l=1;l<columns-1;l++)
	{
	  temp = 
	    sqrt((temp=phi[OFFSET(1,l)]-phi[OFFSET(1,l)],
		  temp*temp*2));
	  if(isnanf(temp)) temp=0.0;

	  py[OFFSET(0,l)] = 1./(temp+ZEE);
	  temp= 
	    sqrt((temp=phi[OFFSET(rows-1,l)]-phi[OFFSET(rows-2,l)],
		  temp*temp*2));
	  if(isnanf(temp)) temp=0.0;    
	  py[OFFSET(rows-1,l)] = 1./(temp+ZEE);
	}
      
      for(k=1;k<rows-1;k++)
	for(l=1;l<columns-1;l++)
	  {
	    delta = DELTA(phi[OFFSET(k,l)])* dt;
	    RHS[OFFSET(k,l)] -= delta*
	      (nu 
	       + lambda1*(u[OFFSET(k,l)]-c1)*(u[OFFSET(k,l)]-c1)
	       + lambda2*(u[OFFSET(k,l)]-c2)*(u[OFFSET(k,l)]-c2));
	    for(j=0;j<MAXNZ;j++)
	      COEF(k*columns+l,j)=0.0;

	    //(phi(i+1)-phi(i))/x(i) - (phi(i)-phi(i-1))/x(i-1)
	    COEF(k*columns+l,3) = -mu*px[OFFSET(k,l-1)]*delta;
	    COEF(k*columns+l,5) = -mu*px[OFFSET(k,l)]*delta;
	    COEF(k*columns+l,1) = -mu*py[OFFSET(k-1,l)]*delta;
	    COEF(k*columns+l,7) = -mu*py[OFFSET(k,l)]*delta;
	    COEF(OFFSET(k,l),4) = 1.0
	      -COEF(k*columns+l,3)
	      -COEF(k*columns+l,5)
	      -COEF(k*columns+l,1)
	      -COEF(k*columns+l,7);
	  }
      /*
      for(i=0;i<N;i++)
	{
	  printf("row %5d",i);
	  for(j=0;j<MAXNZ;j++)
	    printf("%5.3g",COEF(i,j));
	  printf("\n");
	}
      */
      ssorcg_(&N, &NDIM, &MAXNZ, jcoef, coef, RHS, phi,
	      IWKSP, &NW, WKSP, IPARM, RPARM, &IER);
    }

  free(jcoef);
  free(coef);
  free(RHS);
  free(px);
  free(py);
  free(WKSP);
  free(IWKSP);
  printf("t: %d",t);
  return 0;
}
